#include "ActionController.h"
#include <math.h>

int main(int argc, char** argv) {
	Aria::init();

	ArSimpleConnector conn(&argc, argv);
	ArRobot robot;
	ArSonarDevice sonar;

	double maxSpeed=400;
	double stopDistance=300;
	double controlAngle = 50;
	double turnAmount=20;
	double velocityOnTarget=500;
	double velocityNearHurdle=100;
	// Creiamo le istanze delle azioni da usare con il robot
	//parametri della ActionGo: maxSpeed, stopDistance, controlAngle
	ActionGo go(maxSpeed, stopDistance, controlAngle);
	//parametri della ActionTurn: turnThreshold, turnAmount
	ActionTurn turn(stopDistance, turnAmount);

	ActionScouting scout(200, 20);

	//parametri della ActionController: stopDistance, controlAngle, velocityOnTarget, ActionGo *go, ActionTurn *turn
	ActionController controller(stopDistance, controlAngle, velocityOnTarget, velocityNearHurdle, maxSpeed, &go, &turn, &scout);
	ArActionStallRecover recover;

	// Parse all command-line arguments
	if (!Aria::parseArgs()) {
		Aria::logOptions();
		return 1;
	}

	// Connect to the robot
	if (!conn.connectRobot(&robot)) {
		ArLog::log(ArLog::Terse,
				"actionExample: Could not connect to robot! Exiting.");
		return 2;
	}

	// Add the range device to the robot. You should add all the range
	// devices and such before you add actions
	robot.addRangeDevice(&sonar);

	// Add our actions in order. The second argument is the priority,    // with higher priority actions going first, and possibly pre-empting lower
	// priority actions.
	robot.addAction(&recover, 100);
	robot.addAction(&controller, 90);
	robot.addAction(&go,60);
	robot.addAction(&scout, 60);
	robot.addAction(&turn, 70);

	go.deactivate();
	scout.deactivate();
	turn.deactivate();


	// Enable the motors, disable amigobot sounds
	robot.enableMotors();

	// Run the robot processing cycle.
	// 'true' means to return if it loses connection,
	// after which we exit the program.
	robot.run(true);

	Aria::shutdown();
	return 0;
}
